
#include "com_pudu_mydemo_NativeClass.h"
#include <iostream>
#include <string>
#include <vector>

#include "cpp_log.h"
#include "localization.h"
#include "motion_control.h"



using namespace std;


JNIEXPORT jdoubleArray JNICALL Java_com_pudu_mydemo_nativeClass_NativeClass_getSum
  (JNIEnv * env, jclass, jdoubleArray array){

	//return locate_tag values:Vector3d

	vector <Vector3d> uwbs;
	uwbs.push_back (Vector3d (5,5,0));
	uwbs.push_back (Vector3d (-5,5,0));
	uwbs.push_back (Vector3d (5,-5,0));
	uwbs.push_back (Vector3d (-5,-5,0));
	double high = 0.0;
	localization local(uwbs,high);

	vector<double> distance;
	distance.push_back(5*1.0);
	distance.push_back(5*1.414);
	distance.push_back(5*1.414);
	distance.push_back(5*1.414);

	Vector3d current_tag;
	current_tag = local.locate_tag(distance);


	jdouble doubleArray[3];


	if(0 != doubleArray){
		env->GetDoubleArrayRegion(array,0,3,doubleArray);
		doubleArray[0] = current_tag.x();
		doubleArray[1] = current_tag.y();
		doubleArray[2] = current_tag.z();
		env->SetDoubleArrayRegion(array,0,3,doubleArray);  //这里用法不准确
	}

	return array;
}

JNIEXPORT jdouble JNICALL Java_com_pudu_mydemo_nativeClass_NativeClass_getString
  (JNIEnv *env, jclass, jdouble arraylist){
	vector < double > contents;
	for (int i = 0; i < 5; i++) {
		contents.push_back(i+100.0);
	}
	arraylist = contents[2];
	for (int i = 0; i < 5; i++) {
		cout << contents[i] << endl;
	}

	cout << "hello, world\n";
	return arraylist;
}

/*********motion_control.cpp移植************/
/*********motion_control.cpp移植************/
/*********motion_control.cpp移植************/
motion_control * mc=NULL;

JNIEXPORT void JNICALL Java_com_pudu_mydemo_nativeClass_NativeClass_cMotionControl
  (JNIEnv * env, jclass, jdoubleArray path){

	vector <Vector3d> c_path;
	double x;
	double y;
	double z;

	size_t length = env->GetArrayLength(path);
	double *ptr = env->GetDoubleArrayElements(path,0);
	for(size_t i=0;i<length/3;i++){
		x = *(ptr+i*3+0);
		y = *(ptr+i*3+1);
		z = *(ptr+i*3+2);
		c_path.push_back(Vector3d(x,y,z)); //生成路径
	}

	if (mc == NULL) {
		mc = new motion_control(c_path);   //调用构造函数
	} else {
		delete mc;
		mc = new motion_control(c_path);   //调用构造函数
	}
}

/*
 * 一般做法：
 * 先通过GetXXXArrayElements函数把简单类型的数组转化成本地类型的数组，
 * 并返回其数组的指针，然后通过该指针来对拷贝数组进行处理。对拷贝数组处理完后，
 * 通过ReleaseXXXArrayElements函数把修改后的拷贝数组的反射到java数组
 * */
JNIEXPORT jdoubleArray JNICALL Java_com_pudu_mydemo_nativeClass_NativeClass_cAimPoint
  (JNIEnv * env, jclass, jdoubleArray pos,jdoubleArray dir){

	double * pos_ptr = env->GetDoubleArrayElements(pos, 0);
	double * dir_ptr = env->GetDoubleArrayElements(dir, 0);

	Vector3d c_pos(*(pos_ptr+0), *(pos_ptr+1), *(pos_ptr+2));
    Vector3d c_dir(*(dir_ptr+0), *(dir_ptr+1), *(dir_ptr+2));

	Vector3d aim_point = mc->aim_point(c_pos, c_dir);

	jdouble buf[3];
	buf[0] = aim_point.x();
	buf[1] = aim_point.y();
	buf[2] = aim_point.z();

	//生成一个新的数组，防止改变原来数组
	jdoubleArray aimPointReturn = env->NewDoubleArray(3);
	//表示把数组val0从val1开始的val2个元素设置为val3内存地址开始的val2个元素
	env->SetDoubleArrayRegion(aimPointReturn, 0, 3, buf);

	return aimPointReturn;
}

JNIEXPORT void JNICALL Java_com_pudu_mydemo_nativeClass_NativeClass_cSetForesee
  (JNIEnv * env, jclass, jdouble foresee){

	//基本类型，不用使用GetXXXArrayElements等进行转换和映射
	double cForsee = foresee;

	mc->set_foresee(cForsee);
}

JNIEXPORT void JNICALL Java_com_pudu_mydemo_nativeClass_NativeClass_cSetFocal
		(JNIEnv * env, jclass, jdouble a_b){

	double ca_b = a_b;

	mc->set_a_b(ca_b);
}
JNIEXPORT void JNICALL Java_com_pudu_mydemo_nativeClass_NativeClass_cSetSpeed
  (JNIEnv * env, jclass, jdouble speed){

	double cSpeed = speed;

	mc->set_speed(cSpeed);
}

JNIEXPORT void JNICALL Java_com_pudu_mydemo_nativeClass_NativeClass_cSetKp
  (JNIEnv * env, jclass, jdouble Kp){

	double cKp = Kp;

	mc->set_P(cKp);
}

JNIEXPORT void JNICALL Java_com_pudu_mydemo_nativeClass_NativeClass_cSetKd
		(JNIEnv * env, jclass, jdouble Kd){

	double cKd = Kd;

	mc->set_D(cKd);
}

JNIEXPORT jdoubleArray JNICALL Java_com_pudu_mydemo_nativeClass_NativeClass_cTrack
  (JNIEnv * env, jclass, jdoubleArray pos,jdoubleArray dir){

	double * pos_ptr = env->GetDoubleArrayElements(pos,0);
	double * dir_ptr = env->GetDoubleArrayElements(dir,0);

	Vector3d c_pos(*(pos_ptr+0),*(pos_ptr+1),*(pos_ptr+2));
    Vector3d c_dir(*(dir_ptr+0),*(dir_ptr+1),*(dir_ptr+2));

    vector<double> cSpeeds = mc->track(c_pos,c_dir);

    jdouble buf[2];
    buf[0] = cSpeeds[0];
    buf[1] = cSpeeds[1];

    jdoubleArray speedsReturn = env->NewDoubleArray(2);
    env->SetDoubleArrayRegion(speedsReturn,0,2,buf);

    return speedsReturn;
}

/*********Localization.cpp移植************/
/*********Localization.cpp移植************/
/*********Localization.cpp移植************/
localization * lz = NULL;

JNIEXPORT void JNICALL Java_com_pudu_mydemo_nativeClass_NativeClass_cLocalization
  (JNIEnv * env , jclass, jdoubleArray uwbs, jdouble high){

    LOGD("Localization is wroking");
	vector<Vector3d> cUWBs;
	double cHigh = high;
	double x;
	double y;
	double z;

	size_t length = env->GetArrayLength(uwbs);
	double *ptr = env->GetDoubleArrayElements(uwbs,0);
	for(size_t i=0; i < length/3; i++){
		x = *(ptr+i*3+0);
		y = *(ptr+i*3+1);
		z = *(ptr+i*3+2);
		cUWBs.push_back(Vector3d(x,y,z));
	}

		lz = new localization(cUWBs,cHigh);   //调用构造函数
}


JNIEXPORT jdoubleArray JNICALL Java_com_pudu_mydemo_nativeClass_NativeClass_cLocateTag
  (JNIEnv *env , jclass, jdoubleArray distance){

	vector<double> cDistance;

	size_t length = env->GetArrayLength(distance);
	double *ptr = env->GetDoubleArrayElements(distance,0);
	for(size_t i=0; i < length; i++){
		cDistance.push_back(ptr[i]);
	}

	Vector3d cTagPos = lz->locate_tag(cDistance);

	jdouble buf[3];
	buf[0] = cTagPos.x();
	buf[1] = cTagPos.y();
	buf[2] = cTagPos.z();

	jdoubleArray tagPosReturn = env->NewDoubleArray(3);
	env->SetDoubleArrayRegion(tagPosReturn,0,3,buf);

	return tagPosReturn;
}

JNIEXPORT void JNICALL Java_com_pudu_mydemo_nativeClass_NativeClass_cInitPose
  (JNIEnv * env, jclass, jdoubleArray centerPos, jdoubleArray tagPos){

	double * centerPtr = env->GetDoubleArrayElements(centerPos,0);
	double * tagPtr = env->GetDoubleArrayElements(tagPos,0);

	Vector3d cCenterPos(centerPtr[0],centerPtr[1],centerPtr[2]);
	Vector3d cTagPos(tagPtr[0],tagPtr[1],tagPtr[2]);

	lz->init_pose(cCenterPos,cTagPos);
}

JNIEXPORT void JNICALL Java_com_pudu_mydemo_nativeClass_NativeClass_cLocate
  (JNIEnv * env, jclass, jdoubleArray distance){

	vector<double> cDistance;

	size_t length = env->GetArrayLength(distance);
	double *ptr = env->GetDoubleArrayElements(distance,0);
	for(size_t i=0; i < length; i++){
		cDistance.push_back(ptr[i]);
	}

	lz->locate(cDistance);
}

JNIEXPORT jdoubleArray JNICALL Java_com_pudu_mydemo_nativeClass_NativeClass_cGetDir
  (JNIEnv * env, jclass){

	Vector3d cDir = lz->get_dir();

	jdouble buf[3];
	buf[0] = cDir.x();
	buf[1] = cDir.y();
	buf[2] = cDir.z();

	jdoubleArray dirReturn = env->NewDoubleArray(3);
	env->SetDoubleArrayRegion(dirReturn,0,3,buf);

	return dirReturn;
}

JNIEXPORT jdoubleArray JNICALL Java_com_pudu_mydemo_nativeClass_NativeClass_cGetTag
  (JNIEnv * env, jclass){

	Vector3d cTagPos = lz->get_tag();

	jdouble buf[3];
	buf[0] = cTagPos.x();
	buf[1] = cTagPos.y();
	buf[2] = cTagPos.z();

	jdoubleArray tagPosReturn = env->NewDoubleArray(3);
	env->SetDoubleArrayRegion(tagPosReturn,0,3,buf);

	return tagPosReturn;
}

JNIEXPORT jdoubleArray JNICALL Java_com_pudu_mydemo_nativeClass_NativeClass_cGetCenter
  (JNIEnv * env, jclass){

	Vector3d cCenterPos = lz->get_center();

	jdouble buf[3];
	buf[0] = cCenterPos.x();
	buf[1] = cCenterPos.y();
	buf[2] = cCenterPos.z();

	jdoubleArray centerPosReturn = env->NewDoubleArray(3);
	env->SetDoubleArrayRegion(centerPosReturn,0,3,buf);

	return centerPosReturn;
}

JNIEXPORT void JNICALL Java_com_pudu_mydemo_nativeClass_NativeClass_cSetWheelSpeed
  (JNIEnv * env, jclass, jdouble left, jdouble right){

	double cLeft = left;
	double cRight = right;

	lz->set_speed(cLeft,cRight);
}

JNIEXPORT void JNICALL Java_com_pudu_mydemo_nativeClass_NativeClass_cSetWheelSpeed2
  (JNIEnv * env, jclass, jdouble left, jdouble right, jdouble angle, jdouble ratio){

	double cLeft = left;
	double cRight = right;
	double cAngle = angle;
	double cRatio = ratio;

	lz->set_speed_angle(cLeft,cRight,cAngle,cRatio);
}

JNIEXPORT void JNICALL Java_com_pudu_mydemo_nativeClass_NativeClass_cSetNoSigThreshold
  (JNIEnv * env, jclass, jdouble threshold){

	double cThreshold = threshold;

	lz->set_no_sig_threshold(cThreshold);
}

JNIEXPORT void JNICALL Java_com_pudu_mydemo_nativeClass_NativeClass_cSetK
		(JNIEnv * env, jclass, jint k,jdouble value){

	int cK = k;
	double cValue = value;

	lz->set_k(cK,cValue);
}

JNIEXPORT void JNICALL Java_com_pudu_mydemo_nativeClass_NativeClass_cDraw
  (JNIEnv * env, jclass, jdouble scale){

	double cScale = scale;

	lz->draw(cScale);
}

JNIEXPORT void Java_com_pudu_mydemo_nativeClass_NativeClass_cGetLidarData(JNIEnv * env, jclass, jobject lidarData) {

	if(lidarData == NULL)
		return;

	jclass cls_list = env->GetObjectClass(lidarData);
	jmethodID mth_getSize = env->GetMethodID(cls_list, "size", "()I");
	jmethodID mth_get = env->GetMethodID(cls_list, "get", "(I)Ljava/lang/Object;");

	jobject obj = env->CallObjectMethod(lidarData,mth_get,0);
	jclass cls_coordinate = env->GetObjectClass(obj);
	jfieldID value_x = env->GetFieldID(cls_coordinate,"x","D");
	jfieldID value_y = env->GetFieldID(cls_coordinate,"y","D");

	vector<Vector3d> cLidarData;
	int size = env->CallIntMethod(lidarData,mth_getSize);
	for (int i = 0; i < size; i++) {
		jobject obj_lidarPoint = env->CallObjectMethod(lidarData,mth_get,i);
		jdouble x = env->GetDoubleField(obj_lidarPoint,value_x);
		jdouble y = env->GetDoubleField(obj_lidarPoint,value_y);
		cLidarData.push_back(Vector3d(x,y,0));
	}

	mc->getLidarData(cLidarData);
	LOGD("JNI GetLidar %d",size);
}
